﻿//This is partial of FrontMeaMgr.  This part is for storing 1D body depth data of each frame and analyse them to
//get the result front body data after finishing the front measurement.


using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System;
using IMIForUnity;




public partial class FrontMeaMgr : MonoBehaviour {

	public ArrayList AllBody1DDataArr=new ArrayList();
	//This Arraylist is for getting the final average body front data.
	//It will contains and analyse Arrs which contains the depth sample of same index from the middle of Depth1DArray of each frame,
	//And give a average value of each index.

	public ArrayList FinalBodyDepthStoreArr=new ArrayList();
	float[]  FinalBodyDepth1DArray;



//	public float depthMiddle;
//	public float depthM1D;
//	public float depthM1I;
//	public float depthM2D;
//	public float depthM2I;
//	public float depthM3D;
//	public float depthM3I;
//
//
//
//
//
//    public int 	middleIdx;
//	public int  middle1D;
//	public int  middle1I;
//	public int  middle2D;
//	public int  middle2I;
//	public int  middle3D;
//	public int  middle3I;


//	public  Vector3 worldPosArcMiddle=Vector3.zero;
//	public 	Vector3 worldPosEndMin=Vector3.zero;
//	public  Vector3 worldPosEndMax=Vector3.zero;

	public delegate  Vector3 GetWorldPosFunc (int PixPosX, int PixPosY, int depthval);

	public  GetWorldPosFunc   GetWorldPos_by_DepthPixCoord2;
	int MiddleBodyDepthPixCount=0; //Store the final count of body data sampling count.
	//Store the analysed data of each frame to AllBody1DDataArr.
	public void StoreBody1DAnalysedData()
	{
		if(EnableFunction)
		{
			if(( armBodyGapRightIdx>0)&&(armBodyGapLeftIdx>0)&&(armBodyGapRightIdx>armBodyGapLeftIdx))
			{
				int arrlen= (armBodyGapRightIdx-armBodyGapLeftIdx)+1;
				float[] tmpArr=new float[arrlen];

				for(int i=0;i<arrlen;i++)
				{
					tmpArr[i]=Depth1DArray[armBodyGapLeftIdx+i];
				}

				AllBody1DDataArr.Add(tmpArr);
			}
		}

	}



	public void ClearAllBody1DDataArr()
	{
		AllBody1DDataArr.Clear();
		FinalBodyDepthStoreArr.Clear();
		boundaryLeftIdxArr.Clear();
		boundaryYIdxArr.Clear();

		SharpBeginIdxArr.Clear();
		armBodyGapLeftIdxArr.Clear();
		armBodyGapRightIdxArr.Clear();
	}



	public void GetFinalDepthData()
	{
		GetFinalDepthDataRealize();
	}


	void GetFinalDepthDataRealize()
	{
		if(EnableFunction)
		{

		if(AllBody1DDataArr.Count>=3)
		{
			ArrayList SortedBodyDepthArr=new ArrayList();

			//Sort and store all float[] in AllBody1DDataArr to SortedBodyDepthArr
			foreach( float[] floatArr in AllBody1DDataArr)
			{
				if(SortedBodyDepthArr.Count>0)
				{
					bool insertedDuringLoop=false;
					for (int i=0;i<SortedBodyDepthArr.Count;i++)
					{
						if( floatArr.Length<   ((float[])(SortedBodyDepthArr[i])).Length)
						{
							SortedBodyDepthArr.Insert(i, floatArr);
							insertedDuringLoop=true;
							break;
						}
					}

					if(!insertedDuringLoop)
					{
						SortedBodyDepthArr.Insert(SortedBodyDepthArr.Count-1, floatArr);
					}
				}
				else
				{
					SortedBodyDepthArr.Add(floatArr);
				}
			}

			//Remain only the middle 1/3 of float[] store in SortedBodyDepthArr

			//remove the first 1/3;
			//Attention , Do not use SortedBodyDepthArr.Count directly , for it changes.
			int idx1=(SortedBodyDepthArr.Count)/3;
			for(int i=0;i<idx1;i++)
			{
				SortedBodyDepthArr.RemoveAt(0);
			}

			//Attention, Remove the last 1/3 or original arr , meaning remove the last half of the current.
			//Attention , Do not use SortedBodyDepthArr.Count directly , for it changes.

			int idx2=(SortedBodyDepthArr.Count)/2;
			for(int i=0;  i<idx2;  i++)
			{
				SortedBodyDepthArr.RemoveAt(SortedBodyDepthArr.Count-1);
			}


			//Get the middle count of 1D body sample.
			if(SortedBodyDepthArr.Count>0)
			{
				int tmpSum=0;
				for(int i=0;i<SortedBodyDepthArr.Count;i++)
				{
					tmpSum+=  ((float[])(SortedBodyDepthArr[i])).Length;
				}

				MiddleBodyDepthPixCount=tmpSum/SortedBodyDepthArr.Count;

				for(int i=0;i<MiddleBodyDepthPixCount;i++)
				{
					FinalBodyDepthStoreArr.Add(new ArrayList());
				}



				//Store depth in each pixel position to SortedBodyDepthArr
				for(int i=0;i<SortedBodyDepthArr.Count;i++)
				{
					float[] tmpFloatArr=(float[])(SortedBodyDepthArr[i]);
					if(tmpFloatArr.Length>MiddleBodyDepthPixCount)
					{
						int beginIdx=(tmpFloatArr.Length-MiddleBodyDepthPixCount)/2;

						for(int j=0;j<MiddleBodyDepthPixCount;j++)
						{
							((ArrayList)(FinalBodyDepthStoreArr[j])).Add(tmpFloatArr[ beginIdx+j]);
						}
					}
					else if(tmpFloatArr.Length<MiddleBodyDepthPixCount)
					{
						int beginIdx=(MiddleBodyDepthPixCount-tmpFloatArr.Length)/2;
						
						for(int k=0,j=beginIdx;k<tmpFloatArr.Length;j++,k++)
						{
							((ArrayList)(FinalBodyDepthStoreArr[j])).Add(tmpFloatArr[k]);
						}						
					}
					else
					{
						for(int j=0;j<MiddleBodyDepthPixCount;j++)
						{
							((ArrayList)(FinalBodyDepthStoreArr[j])).Add(tmpFloatArr[j]);
						}
					}				
				}

				//get the average value of each entry in FinalBodyDepthStoreArr as the final body depth value in each pixel position.
				FinalBodyDepth1DArray=new float[MiddleBodyDepthPixCount];


				for(int i=0;i<MiddleBodyDepthPixCount;i++)
				{
					FinalBodyDepth1DArray[i]=IMIBodyMeaTool.GetFloatAveOfArraylist( (ArrayList)(FinalBodyDepthStoreArr[i]));
				}
			}
			else
			{
				Debug.LogError("float[] count in SortedBodyDepthArr are smaller than 0");
				
			}

		}
		else
		{

			Debug.LogError("float[] count in AllBody1DDataArr are smaller than 3 which is not enough");
		}


		//Get the final middel-average value of left boundary, left sharp contour and left body boundary.
		boundaryLeftIdx=IMIBodyMeaTool.GetAvgValueOfMiddlePart(boundaryLeftIdxArr);
		boundaryY=IMIBodyMeaTool.GetAvgValueOfMiddlePart(boundaryYIdxArr);
		sharpBeginIdx=IMIBodyMeaTool.GetAvgValueOfMiddlePart(SharpBeginIdxArr);
		armBodyGapLeftIdx=IMIBodyMeaTool.GetAvgValueOfMiddlePart(armBodyGapLeftIdxArr);
		armBodyGapRightIdx=IMIBodyMeaTool.GetAvgValueOfMiddlePart(armBodyGapRightIdxArr);

		//We seperate the 1d sample into 9 points. And we get  5 important points from the 9 sampling, to measure the front length of body.
		//Set the begginning index as 1. the sample is parted to 1/9 to 9/9
		//the samllest part of 2 ends is the smallest value. which is the smaller one of 1/9 and 9/9. If both bogger than 2/9 and 8/9, we set the smallest 2/9 and 8/9 as the smallest one.
		//the center index sample value is the biggest value.  nearly 5/9 not accuately. This is the center.
		//The ave of 4/9 and 6/9 is the 2nd biggest.   middle1D(Decrease)  middle1I(Increase)
		//The ave of 3/9 and 7/9 is the 3nd biggest.   middle2D     middle2I
		//The ave of 2/9 and 8/9 is the 3nd biggest.   middle3D     middle3I
		
		int step0=MiddleBodyDepthPixCount/8;

		int  middleIdx=MiddleBodyDepthPixCount/2;
		int  middle1D=middleIdx-step0;
		int  middle1I=middleIdx+step0;
		int  middle2D=middleIdx-step0*2;
		int middle2I=middleIdx+step0*2;
		int middle3D=middleIdx-step0*3;
		int  middle3I=middleIdx+step0*3;
	   
		float  depthMiddle=baseDepthRulerDist-FinalBodyDepth1DArray[middleIdx];
		float depthM1D=baseDepthRulerDist-FinalBodyDepth1DArray[middle1D];
		float depthM1I=baseDepthRulerDist-FinalBodyDepth1DArray[middle1I];
		float depthM2D=baseDepthRulerDist-FinalBodyDepth1DArray[middle2D];
		float depthM2I=baseDepthRulerDist-FinalBodyDepth1DArray[middle2I];
		float depthM3D=baseDepthRulerDist-FinalBodyDepth1DArray[middle3D];
		float depthM3I=baseDepthRulerDist-FinalBodyDepth1DArray[middle3I];


		//The following several lines of codes is for setting which is the smallest depth value which is for both ends of the body1dIdxes;
		float SampleMin=FinalBodyDepth1DArray[0];
		for(int i=0;i<4;i++)
		{
			if(FinalBodyDepth1DArray[i]<SampleMin)
			{
				SampleMin=FinalBodyDepth1DArray[i];
			}
		}

		for(int i=MiddleBodyDepthPixCount-1;i>MiddleBodyDepthPixCount-5;i--)
		{
			if(FinalBodyDepth1DArray[i]<SampleMin)
			{
				SampleMin=FinalBodyDepth1DArray[i];
			}
		}

		#region If the SampleMin in the end is bigger than the smaller one of the 2 points next to it, than we set the SampleMin as the smaller of the 2 next to it.
		float smallerSampleM3D=FinalBodyDepth1DArray[middle3D];
		if(smallerSampleM3D>FinalBodyDepth1DArray[middle3I])
		{
			smallerSampleM3D=FinalBodyDepth1DArray[middle3I];
		}

		if(SampleMin>smallerSampleM3D)
		{
			SampleMin=smallerSampleM3D;
		}

		#endregion
		//For now se set that The (center sample - boundary sample) must bigger than 220mm. If not, it means that the "gap" we locate is unnecessarily  too dep.
		if((FinalBodyDepth1DArray[middleIdx]-SampleMin)>220)
		{
			SampleMin=FinalBodyDepth1DArray[middleIdx]-220;
		}

		float depthMin=baseDepthRulerDist-SampleMin;


		int wholeBeginIdx=boundaryLeftIdx+sharpBeginIdx+armBodyGapLeftIdx;

			Vector3 worldPosArcMiddle= GetWorldPos_by_DepthPixCoord2(wholeBeginIdx+middleIdx,
		                                                                               boundaryY,
		                                                                               (int)depthMiddle);

			Vector3	 worldPosEndMin= GetWorldPos_by_DepthPixCoord2(wholeBeginIdx,
		                                                                               boundaryY,
		                                                                               (int)depthMin);

			Vector3 worldPosEndMax= GetWorldPos_by_DepthPixCoord2(wholeBeginIdx+MiddleBodyDepthPixCount-1,
		                                                                               boundaryY,
		                                                                               (int)depthMin);

		Vector3 worldPosM1D= GetWorldPos_by_DepthPixCoord2(wholeBeginIdx+middle1D,
		                                                                               boundaryY,
		                                                                               (int)depthM1D);

		Vector3 worldPosM2D= GetWorldPos_by_DepthPixCoord2(wholeBeginIdx+middle2D,
		                                                                            boundaryY,
		                                                                            (int)depthM2D);

		Vector3 worldPosM3D= GetWorldPos_by_DepthPixCoord2(wholeBeginIdx+middle3D,
		                                                                            boundaryY,
		                                                                            (int)depthM3D);

		Vector3 worldPosM1I= GetWorldPos_by_DepthPixCoord2(wholeBeginIdx+middle1I,
		                                                                            boundaryY,
		                                                                            (int)depthM1I);

		Vector3 worldPosM2I= GetWorldPos_by_DepthPixCoord2(wholeBeginIdx+middle2I,
		                                                                            boundaryY,
		                                                                            (int)depthM2I);

		Vector3 worldPosM3I= GetWorldPos_by_DepthPixCoord2(wholeBeginIdx+middle3I,
		                                                                            boundaryY,
		                                                                            (int)depthM3I);


		frontLen0=Vector3.Distance(worldPosEndMin, worldPosM3D)+ 
			    Vector3.Distance(worldPosM3D, worldPosM2D)+ 
				Vector3.Distance(worldPosM2D, worldPosM1D)+
				Vector3.Distance(worldPosM1D, worldPosArcMiddle)+ 
				Vector3.Distance(worldPosArcMiddle, worldPosM1I)+ 
				Vector3.Distance(worldPosM1I, worldPosM2I)+
				Vector3.Distance(worldPosM2I, worldPosM3I)+ 
				Vector3.Distance(worldPosM3I, worldPosEndMax);
				
		frontLen1=Vector3.Distance(worldPosEndMin, worldPosEndMax);
		Vector3 worldPosChordMid=(worldPosEndMin+worldPosEndMax)/2;

		frontSideChordThick=Vector3.Distance(worldPosChordMid, worldPosArcMiddle);


		}
		else
		{
			Debug.LogError("The NetworkCheck failed");
		}
	}



//	public  Vector3 GetWorldPos_by_DepthPixCoord2 (int PixPosX, int PixPosY, int depthval)
//	{
//		ImiWrapper.ImiVector4 _jointPosV4 = new ImiWrapper.ImiVector4 ();
//		ImiWrapper.ConvertDepthPointToSkeletonPoint (
//			PixPosX,
//			PixPosY,
//			depthval,
//			ref _jointPosV4,
//			ImiResolution.Type._640x480);
//		return  new Vector3 (_jointPosV4.x, _jointPosV4.y, _jointPosV4.z);
//	}






	public string GetFinalBodySampleDataString()
	{
		
		if(FinalBodyDepth1DArray!=null)
					{
						if(FinalBodyDepth1DArray.Length>0)
						{
							string depthArrStr=string.Empty;
							int strLen= FinalBodyDepth1DArray.Length;
							int sampleMulti=10;
				int sampleStep= strLen/sampleMulti;
			

				//to exceed the biggest number of FinalBodyDepth1DArray  we must use sampleMulti+2
				for(int i=0;i<(sampleMulti+2);i++)
							{
								int idx=sampleStep*i;
								if(idx>strLen-1)
								{
									idx= strLen-1;
								}
			
			
								depthArrStr=depthArrStr+FinalBodyDepth1DArray[idx].ToString()+"   ";
							}
			
				return depthArrStr;
						}
			
					}

	
		return  "FinalBodyDepth1DArray array is not correct, the sampling data are error";
	}



}
